#include <Arduino.h>
#include <SoftwareSerial.h>

#include "../moco_protocal.h"

// #define DEBUG_PRINT_PLOTTER_DATA
SoftwareSerial sfSerial(2, 3);

void setup()
{

    sfSerial.begin(115200);
    sfSerial.println("System start!");    
    sfSerial.println("Moco start!");

    moco_init(&sfSerial);
    moco_set_gail(MOCO_TX_STAND_RC);
}

void loop()
{
    moco_set_altitude(0.12);
  delay(2000);
  moco_set_altitude(0.9);
  delay(2000);

//     delay(300);
//     moco_recive_data();
//     if (moco_rx_data_state == STA_OK)
//     {
// #ifdef DEBUG_PRINT_PLOTTER_DATA
//         sfSerial.print("$");

//         // Battery percentage
//         sfSerial.print("  battery: ");
//         sfSerial.print(rx_buffer.bat_percent);

//         // Angular
//         sfSerial.print(rx_buffer.pitch);
//         sfSerial.print(" ");
//         sfSerial.print(rx_buffer.roll);
//         sfSerial.print(" ");
//         sfSerial.print(rx_buffer.yaw);
//         sfSerial.print(" ");

//         // Angular acceleration
//         sfSerial.print(rx_buffer.pitch_rate);
//         sfSerial.print(" ");
//         sfSerial.print(rx_buffer.roll_rate);
//         sfSerial.print(" ");
//         sfSerial.print(rx_buffer.yaw_rate);
//         sfSerial.print(" ");

//         // Linear acceleration ? seems not
//         sfSerial.print(rx_buffer.x_acc);
//         sfSerial.print(" ");
//         sfSerial.print(rx_buffer.y_acc);
//         sfSerial.print(" ");
//         sfSerial.print(rx_buffer.z_acc);
//         sfSerial.print(" ");

//         sfSerial.println(";");
// #endif
//         sfSerial.print("motion_group: ");
//         sfSerial.println(moco_rx_data.motion_group);
//     }

//     if(sfSerial.available() > 0)
//     {
//         char c = sfSerial.read();
//         sfSerial.print("recive: ");
//         sfSerial.println(c);

//         if(c == 0) {
//             moco_set_gail(MOCO_TX_TROT);
//         }
//         if(c == 1) {
//             moco_set_gail(MOCO_TX_STAND_RC);
//         }
//         else if(c == 2) {
//             moco_set_gail(MOCO_TX_STAND_IMU);
//         }
//         else if(c == 3) {
//             moco_set_angle(0, 0, 0);
//         }
//         else if(c == 4) {
//             moco_set_angle(20, 0, 0);
//         }
//         else if(c == 5) {
//             moco_set_angle(25, 0, 0);
//         }
//         else if(c == 6) {
//             moco_set_angle(30, 0, 0);
//         }
//         else if(c == 7) {
//             moco_set_angle(0, 0, 0);
//         }
//         else if(c == 8){
//         }
//     }
}
